#include "spline2d.h"
#include "math.h"
#include "interpolation.h"

#include "util/common_macros.h"

using namespace math;

Spline2D::Spline2D() :
	m_Type(ST_CATMULLROM),
	MAX_NODES(50),
	DRAW_SEGMENTS(10),
	TRACK_SEGMENTS(20),
	ASPECT_RATIO(1.0f),
	NODE_HITTEST_SENSITIVITY(0.02f),
	LINE_HITTEST_SENSITIVITY(0.02f),
	m_TrackLen(-1.0f)
{
}

Spline2D::~Spline2D()
{
}

void Spline2D::Init()
{
}

void Spline2D::Clear()
{
	for (unsigned int i = 0; i < m_Nodes.size(); i++)
		DeleteNodeData((PATH_NODE*)m_Nodes[i]);

	m_Nodes.clear();
	m_TrackLen = -1.0f;
}

const bool Spline2D::NodeHitTest(const Vec2& Pos, int* pOut, const float& Tolerance)
{
	Vec2 dir;
	float delta;

	if (Tolerance > 0.0f)
		delta = Tolerance;
	else
		delta = NODE_HITTEST_SENSITIVITY;

	for (int i = 0; i < (int)m_Nodes.size(); i++)
	{
		dir = Pos - m_Nodes[i]->Pos;
		dir.y /= ASPECT_RATIO;

		if (dir.Magnitude() < delta) {
			if (pOut) *pOut = i;
			return true;
		}
	}

	return false;
}

Spline2D::PATH_NODE*	Spline2D::GetNode(int ID) const
{
	if (m_Nodes.empty()) return NULL;
	if (ID < 0 || ID >= (int)m_Nodes.size()) return NULL;
	return m_Nodes[ID];
}

const bool Spline2D::LineHitTest(const Vec2& Pos, int* pOut, Vec2* pVOut, const float& Tolerance)
{
	Vec2 dir, point, tmp, intersect;
	float t, delta;
	int i;
	int NodeC = (int)m_Nodes.size();

	if (NodeC < 2) return false;

	if (Tolerance > 0.0f)
		delta = Tolerance;
	else
		delta = LINE_HITTEST_SENSITIVITY;

	for (i = 0; i < (NodeC - 1); i++)
	{
		// get point and dir of line
		point = m_Nodes[i]->Pos;
		dir = m_Nodes[i+1]->Pos - point;

		// find closest point to line
		tmp = Pos - point;
		t = (dir ^ tmp) / (dir ^ dir);

		if ((t > 0.0f) && (t < 1.0f))
		{
			intersect = point + (t * dir);

			// dist from line
			tmp = Pos - intersect;
			if (tmp.Magnitude() < delta)
			{
				if (pOut) *pOut = i;
				if (pVOut) *pVOut = intersect;
				return true;
			}
		}
	}

	return false;
}

const bool Spline2D::SplineHitTest(const Vec2& Pos, float* pOut, const float& Tolerance)
{
	float s, s_min, t, inv_seg, delta, maxdelta, curdelta, mindelta;
	int i, Idx, NodeC;
	math::Vec2 LinePoint, NextPoint, LineDir, Intersect, tmp;
	math::Vec2 PV1, PV2, PV3, PV4;

	if (Tolerance > 0.0f)
		delta = Tolerance;
	else
		delta = LINE_HITTEST_SENSITIVITY;

	maxdelta = UTIL_LOW( 0.1f * GetTrackLen(), delta * float(TRACK_SEGMENTS) );
	curdelta = delta;
	i = 1;

	// STAGE 1: Find the LINE segment that the point is closest to
	do {
		Idx = -1;
		if (LineHitTest(Pos, &Idx, NULL, curdelta)) break;
		curdelta = float(++i)*delta;

	} while (curdelta < maxdelta);

	if (Idx < 0) return false;

	// STAGE 2: Subdivide the segment and find the closest segment that the point is closest to
	NodeC = (int)m_Nodes.size();
	mindelta = -1.0f;
	inv_seg = 1.0f / float(TRACK_SEGMENTS);

	if (Idx > 0)
		PV1 = m_Nodes[Idx-1]->Pos;
	else
		PV1 = m_Nodes[0]->Pos;

	PV2 = m_Nodes[Idx]->Pos;
	PV3 = m_Nodes[Idx+1]->Pos;

	if ((Idx + 2) >= NodeC)
		PV4 = PV3;
	else
		PV4 = m_Nodes[Idx+2]->Pos;

	Interpolate(LinePoint, PV1, PV2, PV3, PV4, 0.0f);

	for (i = 0; i < (int)TRACK_SEGMENTS; i++)
	{
		// collect data
		s = float(i+1) * inv_seg;
		Interpolate(NextPoint, PV1, PV2, PV3, PV4, s);
		LineDir = NextPoint - LinePoint;

		// get closest dist to line
		tmp = Pos - LinePoint;
		t = (LineDir ^ tmp) / (LineDir ^ LineDir);

		if ((t >= 0.0f) && (t <= 1.0f))
		{
			Intersect = LinePoint + (t * LineDir);
			tmp = Pos - Intersect;
			curdelta = tmp.Magnitude();

			if ((mindelta < 0.0f) || (curdelta < mindelta)) {
				mindelta = curdelta;
				// interpolate to get intersecting 's'
				tmp = Intersect - LinePoint;
				s_min = (float(i) * inv_seg) + (tmp.Magnitude() / LineDir.Magnitude() * inv_seg);
			}
		}

		// next!!!
		LinePoint = NextPoint;
	}

	if ((mindelta < 0.0f) || (mindelta > delta))
		return false;

	if (pOut) {
		// the calculated s_min is normalised to one line segment; now calculate the actual track position
		float s_pos = 0.0f;

		for (i = 0; i < Idx; i++)
			s_pos += m_Nodes[i]->Len;

		s_pos += (s_min * m_Nodes[Idx]->Len);
		*pOut = s_pos;
	}
	return true;
}

const bool Spline2D::SplineHitTest(const Vec2& Pos, Vec2* pOut, const float& Tolerance)
{
	float s;

	if (SplineHitTest(Pos, &s, Tolerance)) {
		if (pOut) *pOut = GetPos(s);
		return true;
	}

	return false;
}

void Spline2D::InsertNode(const Vec2& Pos)
{
	InsertNode(-1, Pos);
}

void Spline2D::InsertNode(int Idx, const Vec2& Pos)
{
	if (m_Nodes.size() >= MAX_NODES) return;

	PATH_NODE* pNode = new PATH_NODE;
	pNode->Pos = Pos;
	pNode->Len = -1.0f;
	pNode->pData = NULL;

	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
	{
		if (Idx > 0) m_Nodes[Idx-1]->Len = -1.0f;
		m_Nodes.insert(m_Nodes.begin() + Idx, pNode);
	}
	else
	{
		if (m_Nodes.size() > 0) m_Nodes[m_Nodes.size() - 1]->Len = -1.0f;
		m_Nodes.push_back(pNode);
	}

	m_TrackLen = -1.0f;
}

void Spline2D::InsertNode(float s, int* pIdx)
{
	UTIL_CLAMP(s, 0.0f, GetTrackLen());

	float len = 0.0f;
	int idx = m_Nodes.size();
	math::Vec2 Pos = GetPos(s);

	for (int i = 0; i < (idx - 1); i++)
	{
		len += m_Nodes[i]->Len;
		if (len > s) {
			idx = i+1;
			break;
		}
	}

	InsertNode(idx, Pos);
	if (pIdx) *pIdx = idx;
}

void Spline2D::DeleteNode(int Idx)
{
	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
	{
		if (Idx > 0) m_Nodes[Idx-1]->Len = -1.0f;

		DeleteNodeData((PATH_NODE*)m_Nodes[Idx]);
		m_Nodes.erase(m_Nodes.begin() + Idx);

		m_TrackLen = -1.0f;
	}
}

void Spline2D::SetNodePos(int Idx, const Vec2& Pos)
{
	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
	{
		m_Nodes[Idx]->Pos = Pos;
		m_Nodes[Idx]->Len = -1.0f;
		m_TrackLen = -1.0f;

		if (Idx > 0) m_Nodes[Idx-1]->Len = -1.0f;
	}
}

const bool Spline2D::GetNodePos(int Idx, Vec2& VOut)
{
	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
	{
		VOut = m_Nodes[Idx]->Pos;
		return true;
	}
	return false;
}

void Spline2D::SetNodeData(int Idx, void* pData)
{
	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
		m_Nodes[Idx]->pData = pData;
}

void* Spline2D::GetNodeData(int Idx)
{
	if ((Idx >= 0) && (Idx < (int)m_Nodes.size()))
		return m_Nodes[Idx]->pData;

	return NULL;
}
	
void Spline2D::RenderLine()
{
	unsigned int NodeC = (unsigned int)m_Nodes.size();
	if (NodeC < 2) return;

	for (unsigned int i = 0; i < (NodeC - 1); i++)
	{
		DrawLine(m_Nodes[i]->Pos, m_Nodes[i+1]->Pos);
		DrawNode(m_Nodes[i]->Pos);
		if (i == (NodeC - 2)) DrawNode(m_Nodes[i+1]->Pos);
	}
}

void Spline2D::RenderSpline()
{
	Vec2 PV1, PV2, PV3, PV4;
	Vec2 From, To;
	float s;
	float inv_segments = 1.0f / float(DRAW_SEGMENTS);
	unsigned int NodeC = (unsigned int)m_Nodes.size();

	if (NodeC < 2) return;

	for (unsigned int i = 0; i < (NodeC - 1); i++)
	{
		if (i == 0)
			PV1 = m_Nodes[i]->Pos;
		else
			PV1 = m_Nodes[i-1]->Pos;

		PV2 = m_Nodes[i]->Pos;
		PV3 = m_Nodes[i+1]->Pos;

		if ((i + 2) >= NodeC)
			PV4 = PV3;
		else
			PV4 = m_Nodes[i+2]->Pos;

		s = 0.0f;
		From = PV2;

		for (int j = 0; j < (int)DRAW_SEGMENTS; j++)
		{
			s = float(j+1) * inv_segments;
			Interpolate(To, PV1, PV2, PV3, PV4, s);
			DrawLine(From, To);
			From = To;
		}
	}
}

Vec2 Spline2D::GetPos(const float& Track, const GET_METHOD& Method)
{
	int NodeC = (int)m_Nodes.size();

	if (NodeC < 2) return math::Vec2(0.0f, 0.0f);
	if (Track <= 0.0f) return m_Nodes[0]->Pos;

	switch (Method)
	{
	case GM_NORMALIZED_LOCAL:

		if (Track < float(NodeC-1))
		{
			int Idx = (int)floor(Track);
			float s = Track - float(Idx);

			if (m_Nodes[Idx]->Len < 0.0f) RefreshTrackLen(Idx);
			s /= m_Nodes[Idx]->Len;

			return GetPos(Idx, s);
		}

	case GM_NORMALIZED_GLOBAL:

		return GetPos(Track * GetTrackLen(), GM_DEFAULT);

	case GM_DEFAULT:
	default:

		if (Track < GetTrackLen())
		{
			float s = Track;
			int i;

			for (i = 0; i < (NodeC - 1); i++) {
				if (m_Nodes[i]->Len < 0.0f) RefreshTrackLen(i);

				if (s < m_Nodes[i]->Len) {
					s /= m_Nodes[i]->Len;
					return GetPos(i, s);
				}
				s -= m_Nodes[i]->Len;
			}
		}
		break;
	}

	// default to return tail position
	return m_Nodes[NodeC-1]->Pos;
}

const float& Spline2D::GetTrackLen()
{
	if (m_TrackLen < 0.0f)
	{
		unsigned int NodeC = (unsigned int)m_Nodes.size();

		m_TrackLen = 0.0f;
		if (NodeC < 2) return m_TrackLen;

		for (unsigned int i = 0; i < (NodeC - 1); i++)
		{
			if (m_Nodes[i]->Len < 0.0f) RefreshTrackLen(i);
			m_TrackLen += m_Nodes[i]->Len;
		}
	}

	return m_TrackLen;
}

void Spline2D::DeleteNodeData(PATH_NODE* pNode)
{
	UTIL_SAFE_DELETE(pNode);
}

void Spline2D::RefreshTrackLen(int Idx)
{
	Vec2 PV1, PV2, PV3, PV4;
	Vec2 From, To, dir;
	float s, inv;
	unsigned int NodeC = (unsigned int)m_Nodes.size();
	float inv_ratio = 1.0f / ASPECT_RATIO;

	inv = 1.0f / float(TRACK_SEGMENTS);
	From = m_Nodes[Idx]->Pos;
	m_Nodes[Idx]->Len = 0.0f;

	if (Idx > 0)
		PV1 = m_Nodes[Idx-1]->Pos;
	else
		PV1 = m_Nodes[0]->Pos;

	PV2 = m_Nodes[Idx]->Pos;
	PV3 = m_Nodes[Idx+1]->Pos;

	if ((Idx + 2) >= (int)NodeC)
		PV4 = PV3;
	else
		PV4 = m_Nodes[Idx+2]->Pos;

	for (int i = 0; i < (int)TRACK_SEGMENTS; i++)
	{
		s = inv * float(i+1);
		Interpolate(To, PV1, PV2, PV3, PV4, s);

		dir = To - From;
		dir.y *= inv_ratio;
		m_Nodes[Idx]->Len += dir.Magnitude();
		From = To;
	}
}

math::Vec2 Spline2D::GetPos(int Idx, const float& s)
{
	int NodeC = (int)m_Nodes.size();
	Vec2 Pos(0.0f, 0.0f);
	Vec2 PV1, PV2, PV3, PV4;

	if (NodeC < 2) return Pos; // not enough nodes to form a spline
	if (Idx < 0) return m_Nodes[0]->Pos; // invalid pos, return head
	if (Idx >= (NodeC - 1)) return m_Nodes[NodeC-1]->Pos; // invalid pos, return tail
	if (s < 0.0f) return m_Nodes[Idx]->Pos; // invalid s, return node
	if (s > 1.0f) return m_Nodes[Idx+1]->Pos; // invalid s, return next node

	if (Idx == 0)
		PV1 = m_Nodes[Idx]->Pos;
	else
		PV1 = m_Nodes[Idx-1]->Pos;

	PV2 = m_Nodes[Idx]->Pos;
	PV3 = m_Nodes[Idx+1]->Pos;

	if ((Idx + 2) >= NodeC)
		PV4 = PV3;
	else
		PV4 = m_Nodes[Idx+2]->Pos;

	Interpolate(Pos, PV1, PV2, PV3, PV4, s);
	return Pos;
}

void Spline2D::Interpolate(Vec2& VOut,
						   const Vec2& PV1, const Vec2& PV2, const Vec2& PV3, const Vec2& PV4,
						   const float& s)
{
	switch (m_Type)
	{
	case ST_CATMULLROM:	// use hermite for now.
		// FIXME: Reenable
		VOut = CathmullRomVec2(PV1, PV2, PV3, PV4, s);
		break;
//		D3DXVec2CatmullRom(&VOut, &PV1, &PV2, &PV3, &PV4, s);
//		break;
	case ST_HERMITE:
		{
			math::Vec2 T1 = (PV3 - PV1) * 0.5f;
			math::Vec2 T2 = (PV4 - PV2) * 0.5f;
		//	D3DXVec2Hermite(&VOut, &PV2, &T1, &PV3, &T2, s);	// replace with internal
			VOut = HermiteVec2(PV2, T1, PV3, T2, s);
		}
		break;
	case ST_LERP:
	default:
//		D3DXVec2Lerp(&VOut, &PV2, &PV3, s);
		VOut = PV2 + ((PV3-PV2) * s);
		break;
	}
}



SplineCache2D::SplineCache2D(const float& interval) : Spline2D(),
	m_Interval(interval),
	m_pCache(NULL),
	m_CacheSize(0)
{
}

SplineCache2D::~SplineCache2D()
{
	UTIL_SAFE_DELETEA(m_pCache);
}

int SplineCache2D::FindCachePos(const Vec2& Pos, int From, int To)
{
	int i, Start, End, MinIdx;
	Vec2 Dir;
	float dist, min_dist;
	float inv_ratio = 1.0f / ASPECT_RATIO;

	if (!m_pCache) RefreshCache();

	Start = ((From >= 0) && (From < m_CacheSize)) ? From : 0;
	if ((To <= Start) || (To > m_CacheSize))
		End = m_CacheSize;
	else
		End = To;

	min_dist = -1.0f;
	MinIdx = -1;

	for (i = Start; i < End; i++)
	{
		Dir = m_pCache[i].pos - Pos;
		Dir.y *= inv_ratio;
		dist = Dir.Magnitude();

		if ((MinIdx < 0) || (dist < min_dist))
		{
			MinIdx = i;
			min_dist = dist;
		}
	}

	return MinIdx;
}

math::Vec2 SplineCache2D::GetCachePos(int Idx)
{
	if (!m_pCache) RefreshCache();

	if ((Idx >= 0) && (Idx < m_CacheSize))
		return m_pCache[Idx].pos;

	return Vec2(0.0f, 0.0f);
}

math::Vec2 SplineCache2D::GetCachePos(int Idx, float Fraction)
{
	math::Vec2	A = GetCachePos(Idx);
	math::Vec2	B = GetCachePos(Idx+1);
	UTIL_CLAMP(Fraction, 0, 1);
	return A + (B-A)*Fraction;
}

math::Vec2 SplineCache2D::GetCachePos(float fIdx)
{
	float f = floor(fIdx);
	if (f == fIdx) return GetCachePos(int(f));

	return GetCachePos(int(f), fIdx - f);
}

float SplineCache2D::GetCacheS(int Idx)
{
	if (!m_pCache) RefreshCache();

	if ((Idx >= 0) && (Idx < m_CacheSize))
		return m_pCache[Idx].s;

	return 0.0f;
}

float SplineCache2D::GetCacheS(float fIdx)
{
	float S1 = GetCacheS((int)floor(fIdx));
	float S2 = GetCacheS((int)ceil(fIdx));
	UTIL_CLAMP(fIdx, 0, 1);
	return S1 + (S2-S1)*fIdx;
}

int SplineCache2D::GetCacheSize()
{
	if (!m_pCache) RefreshCache();
	return m_CacheSize;
}

void SplineCache2D::CacheDirty()
{
	UTIL_SAFE_DELETEA(m_pCache);
	m_CacheSize = 0;
}

void SplineCache2D::RefreshCache()
{
	int i, MaxSize;
	float s, dist;
	Vec2 Pos, Dir, Prev;
	float inv_ratio = 1.0f / ASPECT_RATIO;
	float inv_subdivide = 1.0f / SUBDIVIDE_INTERVAL;

	UTIL_SAFE_DELETEA(m_pCache);
	m_CacheSize = 0;

	if (GetNodeC() < 2) return;

	MaxSize = (int)ceil(GetTrackLen() / m_Interval);
	m_pCache = new CACHE_POINT[MaxSize];

	if (!GetNodePos(0, m_pCache[0].pos)) return;
	m_pCache[0].s = 0.0f;
	m_CacheSize = 1;
	dist = 0.0f;
	Prev = m_pCache[0].pos;

	for (i = 1; i < (SUBDIVIDE_INTERVAL * MaxSize); i++)
	{
		s = inv_subdivide * float(i) * m_Interval; // subdivide for more accuracy
		if (s > GetTrackLen()) break;

		Pos = GetPos(s);
		Dir = Pos - Prev;
		Dir.y *= inv_ratio;
		dist += Dir.Magnitude();

		if (dist >= m_Interval) {
			m_pCache[m_CacheSize].s = s;
			m_pCache[m_CacheSize++].pos = Pos;
			dist -= m_Interval;
		}

		if (m_CacheSize >= MaxSize) break;
		Prev = Pos;
	}
}
